
// Copyright 2009 David Lee Miller
//
// This file is part of POSSM (Python Open Source Scanning Microscopy).
//
// POSSM is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// POSSM is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License         
// along with POSSM.  If not, see <http://www.gnu.org/licenses/>.
//                                                                           
/////////////////////////////////////////////////////////////////////////

#include "Python.h"
#include "comedilib.h"
#include <math.h>
#include <stdio.h>
#include <time.h>
#include <sys/time.h>

#include <string.h>  /* String function definitions */
#include <unistd.h>  /* UNIX standard function definitions */
#include <fcntl.h>   /* File control definitions */
#include <errno.h>   /* Error number definitions */
#include <termios.h> /* POSIX terminal control definitions */

int aref = AREF_GROUND; /* more on this later */
comedi_t *it0;
int devno0 = -1;
int fd = -1;

static PyObject *py_initDevs(PyObject *self, PyObject *args)
{
    char device0[20];

    if (!PyArg_ParseTuple(args, "i",&devno0))
        return NULL;
    if (devno0!=-1) sprintf(device0,"/dev/comedi%d",devno0);
    it0=comedi_open(device0);

    return Py_None;
}

static PyObject *py_initSer(PyObject *self, PyObject *args)
{
    if (!PyArg_ParseTuple(args, "i",&fd))
        return NULL;

    if (fd == -1)
    {
	perror("open_port: Unable to open port.");
    }
    else fcntl(fd, F_SETFL, 0);

    return Py_None;
}

static PyObject *py_microsleep(PyObject *self, PyObject *args)
{
    int useconds;
    if (!PyArg_ParseTuple(args, "i",&useconds))
        return NULL;
    if (useconds>15){
        struct timespec tim1;
        int nanoperiod = (useconds-15)*1000; //to compensate for a ~1us call time into python
        clock_gettime(CLOCK_REALTIME, &tim1);
        tim1.tv_nsec += nanoperiod;
        if(useconds>15) usleep(useconds-15); //use usleep for the bulk of the timing
        clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &tim1, NULL);
    }
    return Py_None;
}

// float readFew(int dev,int subdev,int chan,int range,int nreads)
// {
//     int maxdata,i;
//     comedi_range *crange;
//     float sum=0.0;
//     float tmpval=0.0;
//     comedi_t *it;
//     if(dev==devno0) it=it0;
//     else if(dev==devno1) it=it1;
//     else it=it0;
//     maxdata = comedi_get_maxdata(it,subdev,chan);
//     crange = comedi_get_range(it,subdev,chan,range);
//     if(nreads==1)
//     {
//         lsampl_t data;
//         comedi_data_read(it,subdev,chan,range,aref, &data);
//         sum = comedi_to_phys(data,crange,maxdata);
//         if(sum!=sum&&data==0) sum = crange->min;
// 	else if(sum!=sum&&data==comedi_get_maxdata(it,subdev,chan)) sum = crange->max;
//     }
//     else
//     {
//         lsampl_t data[nreads];
//         comedi_data_read_n(it,subdev,chan,range,aref, data, nreads);
//         for(i=0;i<nreads;i++)
// 	{
// 	    tmpval=comedi_to_phys(data[i],crange,maxdata);
//             if(tmpval!=tmpval&&data[i]==0) tmpval = crange->min;
// 	    else if(tmpval!=tmpval&&data[i]==comedi_get_maxdata(it,subdev,chan)) tmpval = crange->max;
// 	    sum+=tmpval;
// 	}
//     }
//     return sum/nreads;
// }

float readServo(const char *letter)
{
    int n = 0;
    int res = 0;

    tcflush(fd, TCIFLUSH); //flush the lines before writing

    union {
	float flt; 
	char buff[4];
    } var1;

    n = write(fd, letter, 1);
    if (n < 0) fputs("write() failed!\n", stderr);

    usleep(750); // Might try to decrease this number for speed later

    res = read(fd, var1.buff, 4);

    if (res == 0) fputs("read() failed!\n", stderr);
    else tcflush(fd, TCIFLUSH);

    return var1.flt;
}

unsigned short readState(const char *letter)
{
    int n = 0;
    int res = 0;

    tcflush(fd, TCIFLUSH); //flush the lines before writing

    union {
	unsigned short integ; 
	unsigned char buff[2];
    } var1;

    n = write(fd, letter, 1);
    if (n < 0) fputs("write() failed!\n", stderr);

    usleep(750); // Might try to decrease this number for speed later

    res = read(fd, var1.buff, 2);

    if (res == 0) fputs("read() failed!\n", stderr);
    else tcflush(fd, TCIFLUSH);
    return var1.integ;
}

float writeSteps(int dev,int subdev,int chan,int range,int start,int nsteps,int step,int uperiod)
{
    int i;
    int val;
    int res=0;
    struct timespec tim1;
    int nanoperiod = uperiod*1000;
    comedi_t *it;
    if(dev==devno0) it=it0;
    else it=it0;
    clock_gettime(CLOCK_REALTIME, &tim1);
    for(i=0;i<nsteps;i++)
    {
	tim1.tv_nsec += nanoperiod;
        val = start+i*step;
        comedi_data_write(it,subdev,chan,range,aref,val);
        if(uperiod>50) usleep(uperiod-50);
	res = clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &tim1, NULL);
	if (res != 0){
                clock_gettime(CLOCK_REALTIME, &tim1);
		//printf("Failure");
	};
    }
    return start+i*step;
}


static PyObject *py_rwScan(PyObject *self, PyObject *args)
{
    int start,direction,navgs,step,nsteps,microsleep,uperiod,rmpstp;
    int i,j;
    int res=0;
    int tdiff=0;
    const char *letter;
    int wdev,wsubdev,wchan,wrange;
    struct timespec tim1,tim2;

    if (!PyArg_ParseTuple(args, "s(iiii)iiiiiiii",&letter,&wdev,&wsubdev,&wchan,&wrange,
        &start,&nsteps,&direction,&navgs,&step,&rmpstp,&microsleep,&uperiod))
        return NULL;

    int length = (nsteps/navgs);
    PyObject *rlist= PyList_New(length);
    PyObject *wlist= PyList_New(length);
    PyObject *retlist = PyList_New(3);
    float sum=0.0;
    int current_dac = start;
    int maxdata;
    comedi_range *crange;
    comedi_t *it;
    if(wdev==devno0) it=it0;
    else it=it0;
    maxdata = comedi_get_maxdata(it,wsubdev,wchan);
    crange = comedi_get_range(it,wsubdev,wchan,wrange);
/*    int wstep = direction*step;*/
    for(i=0;i<length;i++)
    {
	if (current_dac == 0) PyList_SetItem(wlist, i, Py_BuildValue("f", -10.0)); // hard coded as -10 because crange->min was segfaulting
	else if (current_dac == maxdata) PyList_SetItem(wlist, i, Py_BuildValue("f", 10.0));
        else PyList_SetItem(wlist, i, Py_BuildValue("f", comedi_to_phys(current_dac,crange,maxdata)));
        for(j=0;j<navgs;j++)
        {
            if(i==1) clock_gettime(CLOCK_REALTIME, &tim1);
            else tim1.tv_nsec += tdiff;
            usleep(microsleep);
            sum+=readServo(letter);
            if (rmpstp>step||rmpstp<1) rmpstp=step;
            current_dac = writeSteps(wdev,wsubdev,wchan,wrange,current_dac,rmpstp,direction*(step/(rmpstp)),uperiod);
            if(i==1)
            {
                clock_gettime(CLOCK_REALTIME, &tim2);
                tdiff = tim2.tv_nsec - tim1.tv_nsec;
                tdiff *= 1.2;
                clock_gettime(CLOCK_REALTIME, &tim1);
            }
            else res = clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &tim1, NULL);
            if (res != 0){
                clock_gettime(CLOCK_REALTIME, &tim1);
                //printf("Failure!");
            };
        }
        PyList_SetItem(rlist, i, Py_BuildValue("f", sum/navgs));
        sum = 0.0;
    }
    res=PyList_SetItem(retlist, 0, rlist);
    if(res!=0) printf("Failed to set retlist item 0");
    res=PyList_SetItem(retlist, 1, wlist);
    if(res!=0) printf("Failed to set retlist item 1");
    res=PyList_SetItem(retlist, 2, Py_BuildValue("i",current_dac));
    if(res!=0) printf("Failed to set retlist item 2");
    return retlist;
}

// static PyObject *py_readFew(PyObject *self, PyObject *args)
// {
//     int nreads;
//     float result=0.0;
// 
//     if (!PyArg_ParseTuple(args, "(iiii)i", &dev,&subdev,&chan,&range,&nreads))
//         return NULL;
// 
//     result = readFew(dev,subdev,chan,range,nreads);
//     return Py_BuildValue("f",result);
// }

static PyObject *py_readServo(PyObject *self, PyObject *args)
{
    const char *letter;
    float res = 0.0;

    if (!PyArg_ParseTuple(args, "s",&letter))
        return NULL;

    res = readServo(letter);

    return Py_BuildValue("f",res);
}

static PyObject *py_readState(PyObject *self, PyObject *args)
{
    const char *letter;
    unsigned short res = 0;

    if (!PyArg_ParseTuple(args, "s",&letter))
        return NULL;

    res = readState(letter);

    return Py_BuildValue("i",res);
}

static PyObject *py_writeSteps(PyObject *self, PyObject *args)
{
    int subdev,chan,range,dev;
    int nsteps,step,start,uperiod;
//     float freq;
    int result=0;

    if (!PyArg_ParseTuple(args, "(iiii)iiii", &dev,&subdev,&chan,&range,&start,&nsteps,&step,&uperiod))
        return NULL;

    result = writeSteps(dev,subdev,chan,range,start,nsteps,step,uperiod);
    return Py_BuildValue("i",result);
}

static PyMethodDef comediExtensions_methods[] = {
    {"readServo", py_readServo, METH_VARARGS},
    {"readState", py_readState, METH_VARARGS},
    {"writeSteps", py_writeSteps, METH_VARARGS},
    {"rwScan", py_rwScan, METH_VARARGS},
    {"initDevs", py_initDevs, METH_VARARGS},
    {"initSer", py_initSer, METH_VARARGS},
    {"microsleep", py_microsleep, METH_VARARGS},
    {NULL, NULL}     /* Sentinel - marks the end of this structure */
};

PyDoc_STRVAR(initcomediExtensions__doc__,"Creates the task handle and loading handles for the functions.");

PyMODINIT_FUNC
initcomediExtensions(void)
{
    Py_InitModule3("comediExtensions", comediExtensions_methods,initcomediExtensions__doc__);
}
